/*
 * File:   Turtle.cpp
 * Author: Thomas Trojer
 *
 * Created on 02. Juli 2010, 22:37
 */

#include "turtle/Turtle.h"
#include <iostream>
#include <cmath>

Turtle::Turtle(RoutePlanner& planner) {
    plan = &planner;

    trace = new TurtleState();
    trace->setCode('O');

    state = trace;

    // x,y,z, w
    pos = Vec4f(0, 0, 0, 0);
    dir = Vec4f(0, 1, 0, 0);
    up = Vec4f(0, 0, 1, 0);
}

Turtle::Turtle(const Turtle& orig) {
}

Turtle::~Turtle() {
    // TODO parse the whole tree
    //delete state;
}

void Turtle::watch(Watcher* watcher) {
    watchers.push(watcher);
}

const TurtleState* Turtle::getState() {
    return state;
}

RoutePlanner* Turtle::getPlanner() const {
    return plan;
}

void Turtle::walk() {
    walk(1);
}

void Turtle::walk(float dist) {
    pos = pos + dir * dist;

    std::cout << "\t\t-> Turtle Walk " << pos.x() << "," << pos.y() << "," << pos.z() << std::endl;

    TurtleState* newState = new TurtleState();
    newState->setCode(' ');
    newState->setPosition(pos);
    newState->setDirection(dir);
    newState->setUp(up);

    state->addBranch(*newState);
    TurtleState* oldState = state;
    state = newState;

    for(int i = 0; i < watchers.size(); i++) {
        watchers[i]->look(oldState, state, trace);
    }
}

void Turtle::swim() {
    swim(1);
}

void Turtle::swim(float dist) {
    pos = pos + dir * dist;

    std::cout << "\t\t-> Turtle Swim " << pos.x() << "," << pos.y() << "," << pos.z() << std::endl;
}

void Turtle::roll(float rad) {
    //rotate(up, pos, dir, rad);
	rotate(up, dir, rad);
}

void Turtle::pitch(float rad) {
    Vec3f normal = Vec3f(up.x(), up.y(), up.z()).unitCross(Vec3f(dir.x(), dir.y(), dir.z()));
    normal.normalize();
    /*rotate(dir, pos, Vec4f(normal, 0), rad);
    rotate(up, pos, Vec4f(normal, 0), rad);*/
    rotate(dir, Vec4f(normal, 0), rad);
    rotate(up, Vec4f(normal, 0), rad);
}

void Turtle::rotate(float rad) {
    //rotate(dir, pos, up, rad);
	rotate(dir, up, rad);
}

void Turtle::upright() {
    dir = Vec4f(0, 1, 0, 0);
    up = Vec4f(0, 0, 1, 0);
}

void Turtle::sleep() {
    stack.push(*state);
}

void Turtle::wakeup() {
    if(stack.size() > 0) {
        state = stack.pop();

        pos = Vec4f(*state->getPosition());
        dir = Vec4f(*state->getDirection());
        up = Vec4f(*state->getUp());
    }
}

// feed in rads
void Turtle::rotate(Vec4f& point, const Vec4f& axis, float angle) const {
	double u = axis.x();
	double v = axis.y();
	double w = axis.z();

	double x = point.x();
	double y = point.y();
	double z = point.z();

	double ux = u * x;
	double uy = u * y;
	double uz = u * z;

	double vx = v * x;
	double vy = v * y;
	double vz = v * z;

	double wx = w * x;
	double wy = w * y;
	double wz = w * z;

	double sa = sin(angle);
	double ca = cos(angle);

	point = Vec4f(
			u*(ux+vy+wz)+(x*(v*v+w*w)-u*(vy+wz))*ca+(-wy+vz)*sa,
			v*(ux+vy+wz)+(y*(u*u+w*w)-v*(ux+wz))*ca+(wx-uz)*sa,
			w*(ux+vy+wz)+(z*(u*u+v*v)-w*(ux+vy))*ca+(-vx+uy)*sa,
			point.w()
		);
}



/*void Turtle::rotate(Vec4f& point, const Vec4f& online, const Vec4f& axis, float angle) const {
    angle = 3.14159 * angle/180.0;

    float sn = sin( angle );
    float cs = cos( angle );
    float a = online.x();
    float b = online.y();
    float c = online.z();
    float u = axis.x();
    float v = axis.y();
    float w = axis.z();
    float u2 = pow( u, 2 );
    float v2 = pow( v, 2 );
    float w2 = pow( w, 2 );

    float L2 = u2 + v2 + w2;
    float L = sqrt( L2 );

    if(L2 == 0) {
        std::cout << "L2 too low" << std::endl;
        return;
    }

    float data[] = {
        ( u2 + (v2+w2)*cs ) / L2,       ( u*v*(1-cs) - w*L*sn ) / L2,       ( u*w*(1-cs) - v*L*sn ) / L2,       ( a*(v2+w2)-u*(b*v+c*w)+(u*(b*v+c*w)-a*(v2+w2))*cs+(b*w-c*w)*L*sn ) / L2,
        ( u*v*(1-cs) + w*L*sn ) / L2,   ( v2 + (u2+w2)*cs ) / L2,           ( v*w*(1-cs) + u*L*sn ),            ( b*(u2+w2)-v*(a*u+c*w)+(v*(a*u+c*w)-b*(u2+w2))*cs+(c*u-a*w)*L*sn ) / L2,
        ( u*w*(1-cs) - v*L*sn ) / L2,   ( v*w*(1-cs) + u*L*sn ) / L2,       ( w2 + (u2+v2)*cs ) / L2,           ( c*(u2+v2)-w*(a*u+w*v)+(w*(a*u+w*v)-c*(u2+v2))*cs+(a*v-b*u)*L*sn ) / L2,
        0,                              0,                                  0,                                  1
    };
    Mat4f rot( data );

    float result[3];
    for ( int row = 0; row < 3; row++ ) {
        result[row] = 0;
        for ( int column = 0; column < 3; column++ ) {
            result[row] += point[column] * rot[row][column];
        }
    }

    point = Vec4f( result[0], result[1], result[2], point.w() );
    if(std::isnan(point.x()) || std::isnan(point.y()) || std::isnan(point.z())) {
        std::cout << "ITS NAN!!" << std::endl;
    }
}*/

void Turtle::turn() {
    dir = dir * -1;
}
